#include "calibration.h"
std::ofstream CCalibration::calibrationLog;
void CCalibration::InitLogFile(const std::string& filename) {
        calibrationLog.open(filename, std::ios::out | std::ios::app);
        if (!calibrationLog.is_open()) {
            std::cerr << "打开日志文件失败: " << filename << std::endl;
        }
}
void CCalibration::Log(const std::string& message) {
        if (calibrationLog.is_open()) {
            //std::string log = "["+__DATE__+" "+__TIME__+"] " + "["+__FILE__+":"+__func__+":"+__LINE__+"]:" +  message;
            calibrationLog <<"["<<__DATE__<<":"<<__TIME__<<"] " << "["<<__FILE__<<":"<<__func__<<":"<<__LINE__<<"]:";
            calibrationLog << message << std::endl;
        }
}
void CCalibration::CloseLogFile() {
        if (calibrationLog.is_open()) {
            calibrationLog.close();
        }
}
bool CCalibration::writeParamsTxt()
{
    camK.convertTo(camK, CV_32FC1);
    camDiscoeff.convertTo(camDiscoeff, CV_32FC1);
    ofstream fout(calibResultPath.c_str());//输出详细文件
    if(!fout.is_open())
    {
        //cout<<"文件打开失败："<<calibResultPath<<endl;
        CCalibration::Log("文件打开失败："+calibResultPath);
        return false;
    }

    for(auto& e : errmap)
    {
        fout<<"第"<<e.first<<"幅图像的平均误差为： "<<e.second.first<<"像素"<< ",最大重投影误差：" << e.second.second << "像素" << endl;
    }
    fout<<endl<<"总体平均误差为： "<<avg_err<<"像素"<<endl;
    fout<<endl<<"相机内参矩阵："<<camK<<endl;
    fout<<endl<<"相机畸变矩阵："<< camDiscoeff<<endl;
    fout.close();
    return true;
}

bool CCalibration::writeParamsYaml()
{
    //转换为单精度浮点格式
    camK.convertTo(camK, CV_32FC1);
    camDiscoeff.convertTo(camDiscoeff, CV_32FC1);

    ofstream out(calibResultYaml.c_str());//输出yaml文件
    if(out.is_open() == false)
    {
        CCalibration::Log("文件打开失败："+calibResultYaml);
        //cout<<"文件打开失败："<<calibResultYaml<<endl;
        return false;
    }    
    YAML::Node node;
    node["fx"] = camK.at<float>(0, 0) ;
    node["fy"] = camK.at<float>(1, 1) ;
    node["cx"] =  camK.at<float>(0, 2) ;
    node["cy"] = camK.at<float>(1, 2) ;


// #ifdef CV
    if(camera_type=="CV")
    {
        node["k1"] =  camDiscoeff.at<float>(0, 0);
        node["k2"] = camDiscoeff.at<float>(0, 1) ;
        node["p1"] = camDiscoeff.at<float>(0, 2);
        node["p2"] = camDiscoeff.at<float>(0, 3) ;
        node["k3"] = camDiscoeff.at<float>(0, 4);
    }
// #elif defined FISHEYE
    else if (camera_type=="FISHEYE")
    {
        node["k1"] =  camDiscoeff.at<float>(0, 0) ;
        node["k2"] = camDiscoeff.at<float>(0, 1) ;
        node["u1"] = camDiscoeff.at<float>(0, 2) ;
        node["u2"] = camDiscoeff.at<float>(0, 3) ;
    }
// #endif
    node["image_width"] = imgWidth ;
    node["image_height"] = imgHeight ;
    out << node<<endl;
    out.close();
    return true;
}

bool CCalibration::readPatternImg()
{
    Mat img;
    DIR* dir;
    struct dirent* entry;
    char new_filename[256];
    int imgNum = 0;
    // 打开目录
    dir = opendir(patternImgPath.c_str());
    if (dir == NULL) {
        CCalibration::Log("打开目录失败："+patternImgPath);
       // std::cerr << "打开目录失败: " << patternImgPath << std::endl;
        return false;
    }
    // std::ofstream readjpg_file(iamge_path );
    // if (!readjpg_file.is_open()) {
    //     std::cerr << "打开文件失败: " << iamge_path << std::endl;
    //     return false;
    // }
    while ((entry = readdir(dir)) != NULL) {
        std::string filename(entry->d_name);
        if (filename.find(".jpg") != std::string::npos || filename.find(".png") != std::string::npos) {
           std::string original_path = std::string(patternImgPath) + "/" + filename;
            img = imread(original_path, 0);
            if (img.empty()) {
                CCalibration::Log("读取图片失败："+original_path);
                //std::cerr << "读取图片失败: " << original_path << std::endl;
                continue;
            }
            //readjpg_file << original_path << std::endl;
            patternImgList.push_back(img);
            ++imgNum;
        }
    }
    if (imgNum == 0)
    {
        CCalibration::Log("未找到匹配图片！");
       // cout << " 未找到匹配图片！" << endl;
        return false;
    }
    this->imgNum = imgNum;

    //cout<< patternImgList.size()<<endl;

    imgHeight = patternImgList[0].rows;
    imgWidth = patternImgList[0].cols;
    //readjpg_file.close();
    return true;
}
//通过计算三个相邻角点构成的两个向量之间的夹角判断角点连接性
bool CCalibration::testCorners(vector<cv::Point2f>& corners, int patternWidth, int patternHeight)
{
    if (corners.size() != patternWidth * patternHeight)
    {
        return false;
    }
    double dx1, dx2, dy1, dy2;
    double cosVal;
    for (int i = 0; i < patternHeight; ++i)
    {
        for (int j = 0; j < patternWidth - 2; ++j)
        {
            dx1 = corners[i*patternWidth + j + 1].x - corners[i*patternWidth + j].x;
            dy1 = corners[i*patternWidth + j + 1].y - corners[i*patternWidth + j].y;
            dx2 = corners[i*patternWidth + j + 2].x - corners[i*patternWidth + j + 1].x;
            dy2 = corners[i*patternWidth + j + 2].y - corners[i*patternWidth + j + 1].y;
            cosVal = (dx1 * dx2 + dy1 * dy2) / sqrt((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2));
            if (fabs(cosVal) < 0.993)
            {
                return false;
            }
        }
    }
    for (int i = 0; i < patternHeight - 2; ++i)
    {
        for (int j = 0; j < patternWidth; ++j)
        {
            dx1 = corners[(i + 1)*patternWidth + j].x - corners[i*patternWidth + j].x;
            dy1 = corners[(i + 1)*patternWidth + j].y - corners[i*patternWidth + j].y;
            dx2 = corners[(i + 2)*patternWidth + j].x - corners[(i + 1)*patternWidth + j].x;
            dy2 = corners[(i + 2)*patternWidth + j].y - corners[(i + 1)*patternWidth + j].y;
            cosVal = (dx1 * dx2 + dy1 * dy2) / sqrt((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2));
            if (fabs(cosVal) < 0.993)
            {
                return false;
            }
        }
    }
    return true;
}

//初始化角点的三维坐标
void CCalibration::init3DPoints(cv::Size boardSize, cv::Size squareSize, vector<cv::Point3f> &singlePatternPoint)
{
    for (int i = 0; i<boardSize.height; i++)
    {
        for (int j = 0; j<boardSize.width; j++)
        {
            cv::Point3f tempPoint;//单个角点的三维坐标
            tempPoint.x = float(i * squareSize.width);
            tempPoint.y = float(j * squareSize.height);
            tempPoint.z = 0;
            singlePatternPoint.push_back(tempPoint);
        }
    }
}

void CCalibration::calibProcess()
{
    //***************摄像机标定****************//
    double time0 = (double)getTickCount();
    vector<Point2f> corners;//存储一幅棋盘图中的所有角点二维坐标
    vector<vector<Point2f>> cornersSeq;//存储所有棋盘图角点的二维坐标
    vector<Mat> image_Seq;//存储所有棋盘图
    int successImgNum = 0;
    int count = 0;
    CCalibration::Log("开始提取角点...");
    //cout << "********开始提取角点！********" << endl;
    Mat image, scaleImg;
    for (int i = 0; i<imgNum; i++)
    {
        CCalibration::Log("正在提取第"+std::to_string(i+1)+"幅图像的角点...");
        //cout << "Image#" << i+1 << "......." << endl;
        image = patternImgList[i].clone();
        bool patternfound;
        //降采样原图,加快角点提取速度
        if(downsampling)
        {
            //cout<<"开始降采样"<<endl;
            cv::resize(image, scaleImg, cv::Size(), scale, scale, cv::INTER_LINEAR);
            /**********************提取角点*************************/
            patternfound = findChessboardCorners(scaleImg, boardSize,
                corners, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
            //cout<<"结束降采样"<<endl;
            //patternfound = findChessboardCorners(scaleImg, boardSize,corners);
        }
        else
        {
            patternfound = findChessboardCorners(image, boardSize,corners);//没有降采样
        }

        if (!patternfound)
        {
            CCalibration::Log("第"+std::to_string(i+1)+"幅图像未找到棋盘角点！");
            //cout << "无法找到棋盘角点!\n" << endl;
            continue;
        }
        else
        {
            //上采样corner
            if(downsampling)
            {
                for (int num = 0; num < corners.size(); num++)
                {
                    cv::Point2f tempPoint = corners[num];
                    corners[num] = cv::Point2f(tempPoint.x / scale, tempPoint.y / scale);
                }
            }
           

            /************************亚像素精确化******************************/
            find4QuadCornerSubpix(image, corners, Size(5,5));
            bool good = testCorners(corners, boardSize.width, boardSize.height);
            if (false == good)  continue;

            /************************绘制检测到的角点并显示******************************/
            if(is_draw)
            {
                Mat cornerImg = image.clone();
                cvtColor(cornerImg, cornerImg, cv::COLOR_GRAY2BGR);
                drawChessboardCorners(cornerImg, boardSize, corners, true);//将角点连线--绘制角点
                // for (int j = 0; j< corners.size(); j++)
                // {
                //     circle(cornerImg, corners[j], 20, Scalar(0, 0, 255), 2, 8, 0);
                // }
                namedWindow("CirclePattern", 0);
                imshow("CirclePattern", cornerImg);
                CCalibration::Log( "输入任何键继续...");
                cout << "输入任何键继续..." << endl;
                waitKey(10);
            }
           

            count += (int)corners.size();//所有棋盘图中的角点个数
            successImgNum++;//成功提取角点的棋盘图个数
            cornersSeq.push_back(corners);
            image_Seq.push_back(image);
        }
    }
    CCalibration::Log("角点提取完成！");
    //cout << "*******角点提取完成！******" << endl;

    /**************************摄像机标定******************************/
    Size squre_size = Size(square_size, square_size);//棋盘格尺寸---尺寸不能固定，要根据实际情况调整
    vector<vector<Point3f>> object_points;//所有棋盘图像的角点三维坐标
    vector<int> pointCounts;
    //初始化单幅靶标图片的三维点
    init3DPoints(boardSize, squre_size, singlePatternPoints);
    //初始化标定板上的三维坐标
    for (int n = 0; n<successImgNum; n++)
    {
        object_points.push_back(singlePatternPoints);
        pointCounts.push_back(boardSize.width * boardSize.height);
    }

    /***开始标定***/
    CCalibration::Log("开始标定...");
    //cout << "*****开始标定!******" << endl;
    Size imgSize = Size(imgWidth, imgHeight);
    vector<Vec3d> rotation;//旋转向量
    vector<Vec3d> translation;//平移向量
//#ifdef CV
    if(camera_type=="CV")
    {
        int flags = 0;
        cv::calibrateCamera(object_points, cornersSeq, imgSize, camK, camDiscoeff,
            rotation, translation, flags);
    }
//#elif defined FISHEYE
    else if(camera_type=="FISHEYE")
    {
        int flags = 0;
        cv::fisheye::calibrate(object_points, cornersSeq, imgSize, camK,
            camDiscoeff, rotation, translation, flags, cv::TermCriteria(3, 20, 1e-6));
    }
//#endif
    CCalibration::Log("标定完成！");
    //cout << "*****标定完成！*****" << endl;
    double time1 = getTickCount();
    CCalibration::Log("标定时间:"+std::to_string((time1 - time0) / getTickFrequency())+"s");
    //cout << "Calibration time :" << (time1 - time0) / getTickFrequency() << "s" << endl;
    //评价
    CCalibration::Log("开始评价标定结果...");
    //cout << "*****开始评价标定结果*****" << endl;
    vector<int> outLierIndex;
    evaluateCalibrationResult(object_points, cornersSeq, pointCounts, rotation, translation,
        camK, camDiscoeff, successImgNum, outLierIndex, errThresh);
    //删除误差大的角点图
    vector<vector<cv::Point2f>> newCornersSeq;
    successImgNum = 0;
    for (int i = 0; i < cornersSeq.size(); i++)
    {
        if (outLierIndex[i] == 0)
        {
            newCornersSeq.push_back(cornersSeq[i]);
            successImgNum++;
        }
    }
    vector<vector<cv::Point3f>> newObjectPoints;
    for (int n = 0; n<successImgNum; n++)
    {
        newObjectPoints.push_back(singlePatternPoints);
    }
    CCalibration::Log("评价完成！");
    //cout << "*****评价完成！*****" << endl;
    //删除误差后的重新标定
//     cout << "*****开始重新标定*****" << endl;
// if(camera_type=="CV")
//     {cv::calibrateCamera(newObjectPoints, newCornersSeq, imgSize, camK, camDiscoeff,
//         rotation, translation, flags);
//  }
// else if(camera_type=="FISHEYE"){
//     cv::fisheye::calibrate(newObjectPoints, newCornersSeq, imgSize, camK, camDiscoeff,
//         rotation, translation, flags, cv::TermCriteria(3, 20, 1e-6));
// }
// 
//     //重新计算重投影误差
//     outLierIndex.clear();
//     evaluateCalibrationResult(newObjectPoints, newCornersSeq, pointCounts, rotation, translation,
//         camK, camDiscoeff, successImgNum, outLierIndex, errThresh);
#ifdef DEBUG
    //通过畸变校正效果查看摄像机标定效果
    cv::Mat R = cv::Mat::eye(3, 3, CV_32FC1);
    cv::Mat mapx, mapy, newCamK, undistortImg, showImg;
    cv::initUndistortRectifyMap(camK, camDiscoeff, R, camK, imgSize, CV_32FC1, mapx, mapy);
    cv::remap(image_Seq[0], undistortImg, mapx, mapy, CV_INTER_LINEAR);
    cv::resize(undistortImg, showImg, cv::Size(), 0.25, 0.25, CV_INTER_LINEAR);
    string winName = "undistortImg";
    cv::namedWindow(winName, 1);
    cv::imshow(winName, showImg);
    cv::waitKey(0);
#endif
}

//估计重投影误差，并排除误差大于设定阈值的靶标图片
int CCalibration::evaluateCalibrationResult(vector<vector<cv::Point3f>> objectPoints, vector<vector<cv::Point2f>> cornerSquare, vector<int> pointCnts, vector<cv::Vec3d> _rvec,
    vector<cv::Vec3d> _tvec, cv::Mat _K, cv::Mat _D, int count, vector<int> &outLierIndex, int errThresh)
{
    double total_err = 0.0;//所有图像的平均误差和
    double err = 0.0;//单幅图像的平均误差
    vector<cv::Point2f> proImgPoints;
    for (int i = 0; i< count; i++)
    {
        float maxValue = -1;
        vector<cv::Point3f> tempPointSet = objectPoints[i];
// #ifdef CV
    if(camera_type=="CV")
        cv::projectPoints(tempPointSet, _rvec[i], _tvec[i], _K, _D, proImgPoints);
//#elif defined FISHEYE
    else if(camera_type=="FISHEYE")
        cv::fisheye::projectPoints(tempPointSet, proImgPoints, _rvec[i], _tvec[i], _K, _D);
//#endif

        vector<cv::Point2f> tempImgPoint = cornerSquare[i];
        cv::Mat tempImgPointMat = cv::Mat(1, tempImgPoint.size(), CV_32FC2);
        cv::Mat proImgPointsMat = cv::Mat(1, proImgPoints.size(), CV_32FC2);
        for (int j = 0; j != tempImgPoint.size(); j++)
        {
            proImgPointsMat.at<cv::Vec2f>(0, j) = cv::Vec2f(proImgPoints[j].x, proImgPoints[j].y);
            tempImgPointMat.at<cv::Vec2f>(0, j) = cv::Vec2f(tempImgPoint[j].x, tempImgPoint[j].y);
            float dx = proImgPoints[j].x - tempImgPoint[j].x;
            float dy = proImgPoints[j].y - tempImgPoint[j].y;
            float diff = sqrt(dx*dx + dy*dy);
            if (diff > maxValue)
            {
                maxValue = diff;
            }
        }
        err = norm(proImgPointsMat,  tempImgPointMat, NORM_L2);
        total_err += err;
        err /=  pointCnts[i];
        errmap[i+1] = {err,maxValue};
        //fout << "第" << i << "幅图像的最大重投影误差：" << maxValue << "像素" << endl;

        //找出重投影误差大于errThresh的图
        if (maxValue > errThresh)
        {
            outLierIndex.push_back(-1);
        }
        else
        {
            outLierIndex.push_back(0);
        }
    }
    //总体平均误差：
    avg_err = total_err/count;
    return 0;
}

void CCalibration::run()
{
    bool readSuccess = readPatternImg();
    if (!readSuccess)
    {
        CCalibration::Log("读取棋盘图失败！");
        //cout << "Fail!  No Pattern Imgs !" << endl;
        getchar();
    }
    calibProcess();
    if(!writeParamsYaml())
    {
        CCalibration::Log("写入yaml文件失败！");
        //cout<<"Fail!  No output yaml file!"<<endl;
    }
    if(output_detail)
    {
        if(!writeParamsTxt())
        {
            CCalibration::Log("写入txt文件失败！");
            //cout<<"Fail!  No output txt file!"<<endl;
        }
    }
}